﻿namespace Raven
{
    public static class Transformations
    {
        //--------------------- PointToWorldSpace --------------------------------
        //
        //  Transforms a point from the agent's local space into world space
        //------------------------------------------------------------------------
        public static Vector2 PointToWorldSpace(Vector2 point,
            Vector2 AgentHeading,
            Vector2 AgentSide,
            Vector2 AgentPosition)
        {
            //make a copy of the point
            Vector2 TransPoint = point;

            //create a transformation matrix
            C2DMatrix matTransform = new C2DMatrix();

            //rotate
            matTransform.Rotate(AgentHeading, AgentSide);

            //and translate
            matTransform.Translate(AgentPosition.x, AgentPosition.y);

            //now transform the vertices
            matTransform.TransformVector2s(ref TransPoint);

            return TransPoint;
        }

        //
        //  rotates a vector ang rads around the origin
        //-----------------------------------------------------------------------------
        public static void Vec2DRotateAroundOrigin(ref Vector2 v, Fix ang)
        {
            //create a transformation matrix
            C2DMatrix mat = new C2DMatrix();

            //rotate
            mat.Rotate(ang);

            //now transform the object's vertices
            mat.TransformVector2s(ref v);
        }

        //--------------------- PointToLocalSpace --------------------------------
        //
        //------------------------------------------------------------------------
        public static Vector2 PointToLocalSpace(Vector2 point,
            Vector2 AgentHeading,
            Vector2 AgentSide,
            Vector2 AgentPosition)
        {
            //make a copy of the point
            Vector2 TransPoint = point;

            //create a transformation matrix
            C2DMatrix matTransform = new C2DMatrix();

            Fix Tx = Vector2.Dot(-AgentPosition, AgentHeading);
            Fix Ty = Vector2.Dot(-AgentPosition, AgentSide);

            //create the transformation matrix
            matTransform._11(AgentHeading.x);
            matTransform._12(AgentSide.x);
            matTransform._21(AgentHeading.y);
            matTransform._22(AgentSide.y);
            matTransform._31(Tx);
            matTransform._32(Ty);

            //now transform the vertices
            matTransform.TransformVector2s(ref TransPoint);

            return TransPoint;
        }
    }
}